﻿#include "tcpsocket.h"

#include <QHostAddress>
#include <QDebug>
#include <QThread>
#include <QJsonDocument>
#include <QLabel>
#include <QBuffer>
#include <QMessageBox>


#include<fstream>
#include<iostream>

//R"({"op": "unsubscribe", "topic": "/scan _multi"})";停止订阅话题
//R"({"op": "subscribe", "topic": "/scan _multi" ,"throttle_rate": 10000})";激光雷达数据
//R"({"op": "call_service","service": "/start_mapping","args": {  data: true }})";开始/停止建图
//R"({"op": "call_service","service": "/start_local ization","args": {  filename: "test"}})";启动定位并且选择地图 当filename空为stop
//R"({"op": "call_service","service": "/save_map","args": {  filename: "test"}})";保存创建的地图
//R"({"op": "service_response","service": "/start_loc alization","result": true})";数据回复

//R"({"op": "call_service","service": "/tami_pnc/map_requ est_service","args": {"mode":"1","request": "M_1_dating:LM1"  }})";//导航指令
//发送'{"op": "call_service", "service": "/tami_pnc/map_req uest_service", "args": {"mode": 2,"request":"dataing:LM2"}}'
//返回{"op": "service_response", "service": "/tami_pnc/map_re quest_service", "values": {"success": true, "path_info": "LM1,LM2"}
//订阅话题，当前前往的站点"{"op": "subscribe", "topic": "/navigat ion_status"}"


TcpSocket::TcpSocket(int type,QObject *parent) : QObject(parent)
{
    m_tcp_Type = type;
}

TcpSocket::~TcpSocket()
{
    if(m_socket != NULL)
    {
        m_socket->disconnectFromHost();
        m_socket->deleteLater();
        m_socket = NULL;
    }
}

void TcpSocket::slot_isSubscribe(bool isSub)
{
    if(isSub)
    {
        for(int i=0;i<m_TopicMess.length();i++)
        {
            qDebug()<<"m_TopicMess[i]="<<m_TopicMess[i].TopicName<<m_TopicMess[i].is_Get<<m_TopicMess[i].Topic_Time_Interval;
            if(!m_TopicMess[i].is_Get)
            {
                sendData(QString("R\"({\"op\": \"subscribe\",\"topic\": \"%1\",\"throttle_rate\": %2})\";").arg(m_TopicMess[i].TopicName,QString::number(m_TopicMess[i].Topic_Time_Interval)));
                m_TopicMess[i].is_Get = true;
            }
        }
    }
    else
    {
        for(int i=0;i<m_TopicMess.length();i++)
        {
            if(m_TopicMess[i].is_Get && m_TopicMess[i].TopicName != g_RobotPose)
            {
                sendData(QString("R\"({\"op\": \"unsubscribe\",\"topic\": \"%1\"})\";").arg(m_TopicMess[i].TopicName));
                m_TopicMess[i].is_Get = false;
            }
        }
        QStringList EndDataList;
        emit sig_RevData(g_RobotScan_multi,EndDataList);
    }
}

void TcpSocket::slotTcpRevData()
{
    m_SocketRecvDelay = 0;
    while (m_socket->bytesAvailable() > 0) {
        m_ByteDataTcp += m_socket->readAll();
    }
    //qDebug()<<"TcpSocket::slotTcpRevData="<<m_tcp_Type<<m_ByteDataTcp.length();
    QString  showQstr = QString(m_ByteDataTcp);

    if(!showQstr.startsWith("{"))//找JSON头
    {
        qDebug()<<"showQstr.length"<<showQstr.length();
        if(showQstr.indexOf("}{") != -1)
        {
            showQstr.remove(0,showQstr.indexOf("}{")+1);
            m_ByteDataTcp.remove(0,showQstr.indexOf("}{")+1);
        }
    }


    while((showQstr.startsWith("{") && showQstr.indexOf("}{") != -1)
           || (showQstr.startsWith("{") && showQstr.endsWith("}")))
    {
        QString JsonQstr;
        if(showQstr.startsWith("{") && showQstr.indexOf("}{") != -1)
        {
            JsonQstr = showQstr.left(showQstr.indexOf("}{")+1);
        }
        else if(showQstr.startsWith("{") && showQstr.endsWith("}"))
        {
            JsonQstr = showQstr;
        }

        if(JsonQstr.startsWith("{") && JsonQstr.endsWith("}"))
        {
            showQstr.remove(0,JsonQstr.length());
            m_ByteDataTcp.remove(0,JsonQstr.length());
            QByteArray arr = JsonQstr.toUtf8();
            if(arr.isEmpty())
                return;
            QJsonParseError err;
            QJsonDocument mdocument = QJsonDocument::fromJson(arr, &err);
            if(!mdocument.isEmpty())   // 如果加载json数据失败则打印失败信息
            {

                QVariantMap dataMap;
                dataMap=mdocument.toVariant().toMap();
                qDebug()<< "TcpSocket::slotTcpRevData()" <<dataMap["topic"].toString();
                // if(dataMap["topic"].toString() != g_RobotScan_multi && dataMap["topic"].toString() != g_RobotCreateMap)
                //     qDebug().noquote()<<QDateTime::currentDateTime().toString("hh:mm:ss:zzz")<< PublicShared::variant2JsonStr(dataMap);

                for(int i=0;i<m_TopicMess.length();i++)
                {
                    if(m_TopicMess[i].TopicName == dataMap["topic"].toString())
                        m_TopicMess[i].m_LastRecvDelay = 0;
                }
                if(dataMap["topic"].toString() == g_RobotPose)//机器人点位
                {
                    //qDebug().noquote() << dataMap["topic"].toString() << PublicShared::variant2JsonStr(dataMap);
                    QVariantMap magMap = dataMap["msg"].toMap();
                    QVariantMap poseMap = magMap["pose"].toMap();
                    QVariantMap orientationMap = poseMap["orientation"].toMap();
                    QVariantMap positionMap = poseMap["position"].toMap();
                    if(positionMap["x"].toString().length() > 0 && positionMap["y"].toString().length() > 0
                        && orientationMap["w"].toString().length() > 0
                        && orientationMap["x"].toString().length() > 0
                        && orientationMap["y"].toString().length() > 0
                        && orientationMap["z"].toString().length() > 0)
                    {
                        double DataW = orientationMap["w"].toDouble();
                        double DataX = orientationMap["x"].toDouble();
                        double DataY = orientationMap["y"].toDouble();
                        double DataZ = orientationMap["z"].toDouble();
                        //四元数转欧拉角

                        double yaw = atan2(2.0 * (DataW * DataZ + DataX * DataY), 1.0-2.0 * (DataY * DataY + DataZ * DataZ));//偏向角
                        // double theta = asin(-(2 * (DataZ * DataY - DataW * DataX)));//俯仰角。
                        // double psi = atan2(2 * (DataZ * DataX + DataW * DataY),1 - 2 * (DataX * DataX + DataY * DataY));//翻滚角。

                        double endZ = yaw * 180.0 / M_PI;

                        QStringList dataList;
                        dataList<<positionMap["x"].toString()<<positionMap["y"].toString()<<QString::number(endZ);
                        emit sig_RevData(dataMap["topic"].toString(),dataList);
                    }
                }
                else if(dataMap["topic"].toString() == g_RobotScan_multi && g_RobotScan_multi == "/scan_multi")//激光雷达数据
                {
                    //qDebug().noquote() << dataMap["topic"].toString() << PublicShared::variant2JsonStr(dataMap);
                    QVariantMap magMap = dataMap["msg"].toMap();
                    QVariantList rangesList = magMap["ranges"].toList();
                    int len =  rangesList.length() / 360;
                    QStringList EndDataList;
                    for(int i=0;i<rangesList.length();i=i+len)
                    {
                        EndDataList.append(rangesList[i].toString());
                    }
                    //qDebug()<<"-----------------------------------"<<rangesList.length()<<EndDataList.length();
                    //qDebug()<<"----"<<EndDataList;
                    emit sig_RevData(dataMap["topic"].toString(),EndDataList);
                }
                else if(dataMap["topic"].toString() == g_RobotScan_multi && g_RobotScan_multi == "/scan_multi2")//激光雷达数据
                {
                    //qDebug().noquote() << dataMap["topic"].toString() << PublicShared::variant2JsonStr(dataMap);

                    QVariantMap magMap = dataMap["msg"].toMap();
                    QVariantList rangesList = magMap["ranges"].toList();
                    QVariantList intensitiesList = magMap["intensities"].toList();

                    QStringList EndDataList;
                    for(int i=0;i<rangesList.length();i++)
                    {
                        if(intensitiesList.length() > i)
                        {
                            EndDataList.append(intensitiesList[i].toString() + "=" + rangesList[i].toString());
                        }
                    }
                    emit sig_RevData(dataMap["topic"].toString(),EndDataList);
                }
                else if(dataMap["topic"].toString() == g_RobotBattery_state)//电池信息
                {
                    //qDebug().noquote() << "TcpSocket::slotTcpRevData" << PublicShared::variant2JsonStr(dataMap);
                    QVariantMap magMap = dataMap["msg"].toMap();
                    QStringList dataList;
                    dataList.append(QString::number(magMap["voltage"].toDouble()/1000));
                    dataList.append(QString::number(magMap["current"].toDouble()/100));
                    dataList.append(QString::number(magMap["temperature"].toDouble() - 40));
                    dataList.append(QString::number(magMap["capacity"].toDouble()*100 / magMap["design_capacity"].toDouble()));
                    //dataList.append(QString::number(magMap["percentage"].toInt()));
                    emit sig_RevData(dataMap["topic"].toString(),dataList);
                }
                else if(dataMap["topic"].toString() == g_RobotCmd_vel)//手动控制
                {
                }
                else if(dataMap["topic"].toString() == g_RobotEquipmentStatu)//驱动轮状态
                {
                    //qDebug().noquote() << dataMap["topic"].toString() << PublicShared::variant2JsonStr(dataMap);
                    QVariantMap magMap = dataMap["msg"].toMap();
                    emit sig_RevData(dataMap["topic"].toString(),QStringList()<<magMap["type"].toString()<<magMap["pid"].toString()<<magMap["data"].toString());
                }
                else if(dataMap["topic"].toString() == g_RobotCreateMap)//建图数据
                {
                    QVariantMap magMap = dataMap["msg"].toMap();
                    QVariantMap infoMap = magMap["info"].toMap();
                    g_map_resolution = infoMap["resolution"].toDouble();
                    QVariantMap originMap = infoMap["origin"].toMap();
                    QVariantMap orientationMap = originMap["orientation"].toMap();
                    QVariantMap positionMap = originMap["position"].toMap();
                    g_map_origin[0] = positionMap["x"].toDouble();
                    g_map_origin[1] = positionMap["y"].toDouble();

                    QVariantList dataList = magMap["data"].toList();

                    qDebug()<<"lenth="<<infoMap["width"].toDouble()<<infoMap["height"].toDouble()<<dataList.length() <<g_map_origin[0]<<g_map_origin[1]<<g_map_resolution;
                    DataToPgm(dataList,infoMap["width"].toDouble(),infoMap["height"].toDouble());

                }
                else if(dataMap["topic"].toString() == g_RobotNavi_status)//路线点位信息
                {
                    //qDebug().noquote()<<dataMap["topic"].toString()<< PublicShared::variant2JsonStr(dataMap);

                    QString LogStr = QString(PublicShared::variant2JsonStr(dataMap));
                    emit sig_Loger(LogStr);

                    QVariantMap magMap = dataMap["msg"].toMap();

                    QStringList dataList = magMap["data"].toString().split(u8":");
                    if(dataList.length() > 1)
                    {
                        if(QString(dataList[0]) == "arrived")
                        {
                            QStringList data01List = QString(dataList[1]).split("+");
                            if(data01List.length() > 1)
                            {
                                g_StationPointList = QString(data01List[1]).split(",");
                            }
                            if(QString(data01List[0]).length() > 0)
                            {
                                g_StationOFFPointList.append(QString(data01List[1]));
                                if(data01List[0] == g_CurrentStationPoint)
                                    emit sig_RevData(dataMap["topic"].toString(),QStringList()<<QString(u8"arrivePoint:")+data01List[0]);
                            }
                        }
                        qDebug()<<"g_StationPointList="<<g_StationPointList;
                        qDebug()<<"g_StationOFFPointList="<<g_StationOFFPointList;
                    }
                }
                else if(dataMap["topic"].toString() == g_Robot_linearOrang_speed)//机器人角速度和线速度
                {
                    //qDebug().noquote() << dataMap["topic"].toString() << PublicShared::variant2JsonStr(dataMap);
                    QVariantMap msgMap = dataMap["msg"].toMap();
                    QVariantMap twistMap = msgMap["twist"].toMap();
                    twistMap = twistMap["twist"].toMap();
                    QVariantMap linerMap = twistMap["linear"].toMap();
                    QVariantMap angularMap = twistMap["angular"].toMap();
                    QString lineSpeed,angSpeed;
                    lineSpeed.sprintf("%.2f", linerMap["x"].toDouble());
                    angSpeed.sprintf("%.2f", angularMap["z"].toDouble());

                    QString data = QString("line:%1,ang:%2").arg(lineSpeed,angSpeed);
                    emit sig_RevData(dataMap["topic"].toString(),QStringList()<<data);
                }
                else if(dataMap["topic"].toString() == g_Robot_localization_accuracy)//定位精度
                {
                    //qDebug().noquote() << dataMap["topic"].toString() << PublicShared::variant2JsonStr(dataMap);
                    QVariantMap msgMap = dataMap["msg"].toMap();
                    emit sig_RevData(dataMap["topic"].toString(),QStringList()<<msgMap["data"].toString());
                }
                else
                {
                    qDebug().noquote() << "TcpSocket::slotTcpRevData" << PublicShared::variant2JsonStr(dataMap);
                }


                if(dataMap["op"].toString() == "service_response")//服务回复
                {
                    qDebug().noquote()<< PublicShared::variant2JsonStr(dataMap);
                    QString LogStr = QString(PublicShared::variant2JsonStr(dataMap));
                    emit sig_Loger(LogStr);

                    QVariantMap valuesMap = dataMap["values"].toMap();
                    if(dataMap["service"].toString() == g_RobotLocalization)//开始重定位地图
                    {
                        QString message = valuesMap["message"].toString();
                        if(message.indexOf(" ") == -1)
                        {
                            emit sig_RevData(dataMap["service"].toString(),QStringList()<<message);
                        }
                        else if(message.startsWith("Switched to localization mode with map:"))
                        {
                            message.remove(0,message.lastIndexOf("/")+1);
                            message = message.left(message.indexOf("."));
                            emit sig_RevData(dataMap["service"].toString(),QStringList()<<message);
                        }
                    }
                    else if(dataMap["service"].toString() == g_RobotRequest_service)//导航执行执行状态反馈
                    {
                        if(valuesMap["success"].toBool())
                        {
                            emit sig_Loger("g_CurrentStationPoint="+g_CurrentStationPoint);
                            if(valuesMap["type"].toInt() == 2)
                            {
                                g_StationPointList = valuesMap["path_info"].toString().split(",");
                                qDebug()<<"g_StationPointList="<<g_StationPointList;
                                g_StationOFFPointList.clear();
                                emit sig_RevData(dataMap["service"].toString(),QStringList()<<QString(u8"goPoint:")+g_StationPointList[g_StationPointList.length()-1]);
                                g_CurrentStationPoint = g_StationPointList[g_StationPointList.length()-1];
                            }
                            else if(valuesMap["type"].toInt() == 3)
                            {
                                emit sig_RevData(dataMap["service"].toString(),QStringList()<<QString(u8"stop Navigation task"));
                            }
                            else if(valuesMap["type"].toInt() == 4)
                            {
                                emit sig_RevData(dataMap["service"].toString(),QStringList()<<QString(u8"continue Navigation task"));
                            }
                            else if(valuesMap["type"].toInt() == 5)
                            {
                                g_StationPointList.clear();
                                g_StationOFFPointList.clear();
                                g_CurrentStationPoint = "";
                                emit sig_RevData(dataMap["service"].toString(),QStringList()<<QString(u8"cancel Navigation task"));
                            }
                        }

                        else
                        {
                            QString errorStr = valuesMap["path_info"].toString().length() > 0 ? valuesMap["path_info"].toString() : valuesMap["values"].toString();
                            emit sig_RevData(dataMap["service"].toString(),QStringList()<< errorStr);
                        }
                    }
                }
            }
        }

    }


}

bool TcpSocket::connectToHost(QString ip,int port)
{
    qDebug()<<"connectToHost"<<QThread::currentThreadId();
    qDebug()<<"TcpSocket::connectToHost="<<ip<<port;
    m_Ip = ip;m_Port = port;
    m_ByteDataTcp.clear();
    m_TopicMess.clear();
    emit sig_Loger("TcpSocket::connectToHost="+ip);
    if(ip.isEmpty())
    {
        if(m_Timer != NULL)
            m_Timer->stop();
        if(m_socket != NULL)
        {
            m_socket->disconnectFromHost();
            m_socket->deleteLater();
            m_socket = NULL;
        }
        return false;
    }
    else
    {
        if(NULL != m_socket)
        {
            m_socket->disconnectFromHost();
            m_socket->deleteLater();
            m_socket = NULL;
        }
        m_socket = new QTcpSocket(this);
        m_socket->connectToHost(QHostAddress(ip),port);
        if(!m_socket->waitForConnected(1000))
        {
            qDebug()<<QString(u8"连接服务器失败!\n");
            emit sig_inConnect(false);
            return false;
        }
        m_SocketRecvDelay = 0;
        while (m_socket->bytesAvailable() > 0) {
            m_socket->readAll();
        }
        connect(m_socket, SIGNAL(readyRead()), this, SLOT(slotTcpRevData()));
        if(NULL == m_Timer)
        {
            m_Timer = new QTimer;
            connect(m_Timer,&QTimer::timeout,this,&TcpSocket::slot_Timer);
        }
        m_Timer->start(5000);

        {//订阅位置和电量

            TopicMess mess;
            mess.is_Get = false;
            mess.m_LastRecvDelay = 0;

            mess.Topic_Time_Interval = 1000;
            mess.TopicName = g_RobotPose;
            m_TopicMess.append(mess);
            mess.Topic_Time_Interval = 2000;
            mess.TopicName = g_RobotBattery_state;
            m_TopicMess.append(mess);
            mess.Topic_Time_Interval = 1000;
            mess.TopicName = g_RobotScan_multi;
            m_TopicMess.append(mess);
            mess.Topic_Time_Interval = 0;
            mess.TopicName = g_RobotEquipmentStatu;
            m_TopicMess.append(mess);
            mess.Topic_Time_Interval = 0;
            mess.TopicName = g_RobotNavi_status;
            m_TopicMess.append(mess);
            mess.Topic_Time_Interval = 2000;
            mess.TopicName = g_Robot_localization_accuracy;
            m_TopicMess.append(mess);
            mess.Topic_Time_Interval = 1000;
            mess.TopicName = g_Robot_linearOrang_speed;
            m_TopicMess.append(mess);

            slot_isSubscribe(true);

            QTimer::singleShot(1000,this,[this](){
                sendData(QString("{\"op\": \"call_service\", \"service\": \"%1\", \"args\": {\"filename\": \"\"}};").arg(g_RobotLocalization));
            });
        }

        emit sig_inConnect(true);
        return true;
    }

}

QString TcpSocket::on_controlMove(QString key)
{
    qDebug()<<"TcpSocket::on_controlMove="<<key;
    // Map for movement keys
    std::map<std::string, std::vector<float>> moveBindings
    {
        {"front", {1, 0, 0, 0}},
        {"rightfront", {1, 0, 0, -1}},
        {"left", {0, 0, 0, 1}},
        {"right", {0, 0, 0, -1}},
        {"leftfront", {1, 0, 0, 1}},
        {"leftback", {-1, 0, 0, -1}},
        {"back", {-1, 0, 0, 0}},
        {"rightback", {-1, 0, 0, 1}},
        {"stop", {0, 0, 0, 0}},
    };
    std::map<std::string, std::vector<float>> speedBindings
    {
        {"q", {1.1, 1.1}},
        {"z", {0.9, 0.9}},
        {"w", {1.1, 1}},
        {"x", {0.9, 1}},
        {"e", {1, 1.1}},
        {"c", {1, 0.9}}
    };
    float speed = 0.5; // Linear velocity (m/s)
    float turn = 0.5; // Angular velocity (rad/s)
    float x = 0, y = 0, z = 0, th = 0; // Forward/backward/neutral direction vars


    std::string keyData = key.toStdString();

    // Get the pressed key
    // If the key corresponds to a key in moveBindings
    if (moveBindings.count(keyData) == 1)
    {
        // Grab the direction data
        x = moveBindings[keyData][0];
        y = moveBindings[keyData][1];
        z = moveBindings[keyData][2];
        th = moveBindings[keyData][3];
    }
    // Otherwise if it corresponds to a keyData in speedBindings
    else if (speedBindings.count(keyData) == 1)
    {
        // Grab the speed data
        speed = speed * speedBindings[keyData][0];
        turn = turn * speedBindings[keyData][1];
    }

    // Otherwise, set the robot to stop
    else
    {
        x = 0;
        y = 0;
        z = 0;
        th = 0;
    }
    // Update the Twist message
    // twist.linear.x = x * speed;
    // twist.linear.y = y * speed;
    // twist.linear.z = z * speed;

    // twist.angular.x = 0;
    // twist.angular.y = 0;
    // twist.angular.z = th * turn;

    QVariantMap dataMap;
    dataMap["topic"] = g_RobotCmd_vel;
    dataMap["op"] = "publish";
    QVariantMap msgMap;
    QVariantMap angulardata;
    angulardata["x"] = 0;
    angulardata["y"] = 0;
    angulardata["z"] = th * turn;
    QVariantMap lineardata;
    lineardata["x"] = x * speed;
    lineardata["y"] = y * speed;
    lineardata["z"] = z * speed;
    msgMap["linear"] = lineardata;
    msgMap["angular"] = angulardata;
    dataMap["msg"] = msgMap;

    QJsonDocument jsonDoc = QJsonDocument::fromVariant(dataMap);
    QByteArray byteArray = jsonDoc.toJson(QJsonDocument::Compact);
    //qDebug().noquote() << "TcpSocket::on_controlMove=" << PublicShared::variant2JsonStr(dataMap);

    return QString(byteArray);
}

QString TcpSocket::on_SendXYZMove(QString data)
{
    QStringList dataList = data.split(",");
    if(dataList.length() >= 2)
    {
        QVariantMap dataMap;
        dataMap["topic"] = "/move_base_simple/goal";
        dataMap["op"] = "publish";
        QVariantMap msgMap;
        QVariantMap positiondata;
        positiondata["x"] = QString(dataList[0]).toFloat();
        positiondata["y"] = QString(dataList[1]).toFloat();
        positiondata["z"] = 0;
        QVariantMap orientationdata;
        orientationdata["x"] = 0;
        orientationdata["y"] = 0;
        orientationdata["z"] = 0;
        orientationdata["w"] = 0;

        msgMap["position"] = positiondata;
        msgMap["orientation"] = orientationdata;
        dataMap["msg"] = msgMap;

        QJsonDocument jsonDoc = QJsonDocument::fromVariant(dataMap);
        QByteArray byteArray = jsonDoc.toJson(QJsonDocument::Compact);
        qDebug().noquote() << "TcpSocket::on_SendXYZMove=" << PublicShared::variant2JsonStr(dataMap);

        return QString(byteArray);
    }
    return "";
}

void TcpSocket::slot_SocketSendData(QString Type,QString data)
{
    if(Type == "ControlMove")
    {
        data = on_controlMove(data);
    }
    else if(Type == "SendXYZMove")
    {
        data = on_SendXYZMove(data);
    }
    else if(Type == "ControlBkWarn")
    {
        QVariantMap dataMap;
        dataMap["topic"] = "/controlBkDrive";
        dataMap["op"] = "publish";
        QVariantMap msgMap;
        msgMap["data"] = "86";
        dataMap["msg"] = msgMap;

        QJsonDocument jsonDoc = QJsonDocument::fromVariant(dataMap);
        QByteArray byteArray = jsonDoc.toJson(QJsonDocument::Compact);
        data = QString(byteArray);
    }

    else if(Type == "startCreateMap")
    {
        data = "R\"({\"op\": \"call_service\",\"service\": \"/start_mapping\",\"args\": {\"data\": true}})\";";
    }
    else if(Type == "stopCreateMap")
    {
        data = "R\"({\"op\": \"call_service\",\"service\": \"/start_mapping\",\"args\": {\"data\": false}})\";";
    }
    else if(Type == "saveCreateMap")
    {
        data = QString("R\"({\"op\": \"call_service\",\"service\": \"/save_map\",\"args\": {\"filename\": \"%1\"}})\";").arg(data);
    }

    else if(Type == "StratSubscribe")
    {
        slot_isSubscribe(true);
        return;
    }
    else if(Type == "StopSubscribe")
    {
        slot_isSubscribe(false);
        return;
    }


    sendData(data);
}


void TcpSocket::sendData(QString Msg)
{
    qDebug()<<u8"TcpSocket::sendData:"<<Msg;
    emit sig_Loger(QString(u8"TcpSocket::sendData:%1").arg(Msg));
    if(m_socket == NULL)
        return;
    QString sendMsg = Msg;
    char sendMsgChar[300] = {0};
    strcpy(sendMsgChar, sendMsg.toStdString().c_str());
    int sendRe = m_socket->write(sendMsgChar, sendMsg.length());

    m_socket->waitForBytesWritten(1000);
    m_socket->flush();
    if(sendRe == -1)
    {
        qDebug()<<QString(u8"向服务端发送数据失败！");
        emit sig_RevData("TcpError",QStringList()<<QString(u8"底盘连接异常，已尝试重连!"));
        connectToHost(m_Ip,m_Port);
        return;

    }else
    {
        qDebug()<<QString(u8"向服务端发送数据成功！");
    }
}

void TcpSocket::slot_Timer()
{
    // m_SocketRecvDelay++;
    // if(m_SocketRecvDelay % 4 == 0)
    // {
    //     emit sig_RevData("TcpError",QStringList()<<QString(u8"底盘连接异常，已尝试重连!"));
    //     connectToHost(m_Ip,m_Port);
    // }

    for(int i=0;i<m_TopicMess.length();i++)
    {
        if(!m_TopicMess[i].is_Get)
            continue;

        if(++m_TopicMess[i].m_LastRecvDelay > 2)
        {
            if(m_TopicMess[i].TopicName == g_RobotNavi_status)
            {

            }
            else
            {
                sendData(QString("R\"({\"op\": \"unsubscribe\",\"topic\": \"%1\"})\";").arg(m_TopicMess[i].TopicName));
                sendData(QString("R\"({\"op\": \"subscribe\",\"topic\": \"%1\",\"throttle_rate\": %2})\";").arg(m_TopicMess[i].TopicName,QString::number(m_TopicMess[i].Topic_Time_Interval)));
                if(m_TopicMess[i].TopicName == g_RobotEquipmentStatu)
                    emit sig_RevData(g_RobotEquipmentStatu,QStringList()<<"bkDriveStatu"<<"581582"<<"48");
            }
            m_TopicMess[i].m_LastRecvDelay = 0;
        }
    }
}

void TcpSocket::DataToPgm(QVariantList dataList, int width, int height)
{
    qDebug()<<"TcpSocket::DataToPgm"<<width<<height<<dataList.length();
    int size = dataList.length();
    int8_t *data = new int8_t[size];
    int offset = 0;
    for (const QVariant &variant : dataList) {
        data[offset++] = variant.toInt();
    }
    displayPGM(data,width,height,255);
    // 清理分配的内存
    delete[] data;
}

void TcpSocket::displayPGM(int8_t *data, int width, int height, int maxValue)
{
    int threshold_occupied = 65;
    int threshold_free = 25;

    std::string mapdatafile = g_strExeRoot.toStdString()+"/createMap.pgm";
    FILE* out = fopen(mapdatafile.c_str(), "w");
    if (!out)
    {
        return;
    }
    fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n",
            g_map_resolution, width, height);
    for(unsigned int y = 0; y < height; y++) {
        for(unsigned int x = 0; x < width; x++) {
            unsigned int i = x + (height - y - 1) * width;
            if (data[i] >= 0 && data[i] <= threshold_free) { // [0,free)
                fputc(254, out);
            } else if (data[i] >= threshold_occupied) { // (occ,255]
                fputc(000, out);
            } else { //occ [0.25,0.65]
                fputc(205, out);
            }
        }
    }

    fclose(out);
    QPixmap SavePixmap(g_strExeRoot+"/createMap.pgm");
    emit sig_mapTopicPgm(SavePixmap);

    return;
    QImage image(width, height, QImage::Format_Grayscale8);
    // 填充索引颜色表
     QVector<QRgb> colorTable;
    for (int i = 0; i < 256; i++) {
        colorTable.push_back(qRgb(i, i, i));
    }
    image.setColorTable(colorTable);

    // 将数据复制到QImage
    for (int y = 0; y < height; y++) {
        for (int x = 0; x < width; x++) {
            //image.setPixel(x, y, data[y * width + x]);
            // 确保数据在0-255范围内
            uint8_t value = qBound(uint8_t(0), static_cast<uint8_t>(data[y * width + x]), uint8_t(255));
            image.setPixel(x, y, qRgb(value, value, value));
        }
    }
    // 转换为PGM格式
    QByteArray pgmHeader = QString("P5\n%1 %2\n%3\n").arg(width).arg(height).arg(maxValue).toLatin1();
    QFile file("output.pgm");
    file.open(QIODevice::WriteOnly);
    file.write(pgmHeader);
    file.write((const char*)data, width * height);
    file.close();
    // 显示图像
    QPixmap pixmap = QPixmap::fromImage(image);
    emit sig_mapTopicPgm(pixmap);
}

void TcpSocket::savePGM(const std::string &filename, int8_t *data, int width, int height, int max_value)
{
    std::ofstream file(filename, std::ios::binary);
    if (!file) {
        std::cerr << "Cannot open file " << filename << " for writing." << std::endl;
        return;
    }
    file << "P5" << std::endl; // Portable PGM format
    file << width << " " << height << std::endl;
    file << max_value << std::endl;
    // Write the pixel data
    file.write(reinterpret_cast<const char*>(data), width * height);
    file.close();
}
